//////////////////////////////////////////////////////////////////////
//
//  CryEngine Source code
//	
//	File:Skeleton.cpp
//  Implementation of Skeleton class (Forward Kinematics)
//
//	History:
//	January 12, 2005: Created by Ivo Herzeg <ivo@crytek.de>
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

#include <CryExtension/Impl/ClassWeaver.h>
#include <CryExtension/Impl/ICryFactoryRegistryImpl.h>
#include <CryExtension/CryCreateClassInstance.h>

#include <I3DEngine.h>
#include <IRenderAuxGeom.h>

#include "CharacterManager.h"
#include "CharacterInstance.h"
#include "Model.h"
#include "ModelSkeleton.h"

#include "Helper.h"
#include "ControllerOpt.h"

#include "PoseModifier/Recoil.h"

#include "PoseModifier/LookAt.h"

#define NO_PUSHED_LOOKPOSE 65535

CSkeletonPose::CSkeletonPose()
{
	m_pPoseDataWriteable = NULL;
	m_bGroundAlign = false;
	
	m_lastPushedLookPose = NO_PUSHED_LOOKPOSE;

	memset(&m_auxPhys,0, sizeof(m_auxPhys));
	m_bSetDefaultPoseExecute = false;
	// LookIK Disabled
	m_LookIKLayer = numVIRTUALLAYERS;
	m_pPhysUpdateValidator = 0;
	
	::CryCreateClassInstance<IAnimationPoseModifier>("AnimationPoseModifier_LookAt", m_LookAt);
	assert(m_LookAt.get());
}

CSkeletonPose::~CSkeletonPose() 
{ 
	if (m_pPhysUpdateValidator) 
	{ 
		{ WriteLock loc(m_pPhysUpdateValidator->lock); 
			m_pPhysUpdateValidator->bValid = 0;
		}
		m_pPhysUpdateValidator->Release(); 
	} 
}

//////////////////////////////////////////////////////////////////////////
// initialize the moving skeleton 
//////////////////////////////////////////////////////////////////////////
void CSkeletonPose::InitSkeletonPose(CCharInstance* pInstance, CSkeletonAnim* pSkeletonAnim)
{
	m_pInstance = pInstance;
	m_pSkeletonAnim = pSkeletonAnim;
	m_pModelSkeleton = &pInstance->m_pModel->m_ModelSkeleton;
	m_parrModelJoints = &pInstance->m_pModel->m_ModelSkeleton.m_arrModelJoints[0];

	m_poseData.Initialize(m_pInstance->m_pModel->m_ModelSkeleton);
	m_pPoseDataDefault = &pInstance->m_pModel->m_ModelSkeleton.m_poseData;
	
	//usually not needed for skin-attachments
	m_LookIK().m_addHead.SetIdentity();
	m_LookIK().m_addLEye.SetIdentity();
	m_LookIK().m_addREye.SetIdentity();

	//------------------------------------------------------------
	m_nForceSkeletonUpdate=0;
	m_bGroundAlignment = (Console::GetInst().ca_GroundAlignment != 0);

	m_pPostProcessCallback = 0;	
	m_pPostProcessCallbackData = 0;

	//---------------------------------------------------------------------
	//---                   physics                                    ----
	//---------------------------------------------------------------------
	m_pCharPhysics = 0;
	m_bHasPhysics = 0;
	m_timeStandingUp = -1.0f;
	m_ppBonePhysics = 0;
	m_timeLying = -1.0f;
	m_timeNoColl = -1.0f;
	m_timeStandingUp = -1.0f;
	m_fScale = 0.01f;
	m_bPhysicsAwake = 0;
	m_bPhysicsWasAwake = 1;
	m_nAuxPhys = 0;
	m_fPhysBlendTime = 1E6f;
	m_fPhysBlendMaxTime = 1.0f;
	m_frPhysBlendMaxTime = 1.0f;
	m_bPhysicsRelinquished = 0;
	m_bSetRagdollDefaultPoseRequested = 0;
	m_iFnPSet = 0;
	m_vOffset(0,0,0);
	m_velPivot.zero(); m_prevPosPivot.zero();
	m_nPhysJoints = -1;

	m_AABB.min=Vec3(-2,-2,-2);	
	m_AABB.max=Vec3(+2,+2,+2);	
	m_Superimposed=0;

	SetDefaultPoseExecute(false);

	InitializeAnimToPhysIndexArray();

	InitPhysicsSkeleton();
}

Skeleton::CPoseData* CSkeletonPose::GetPoseDataWriteable()
{	
	return m_pPoseDataWriteable;
}

Skeleton::CPoseData& CSkeletonPose::GetPoseDataForceWriteable()
{
	m_pSkeletonAnim->FinishAnimationComputations();
	return m_poseData;
}

bool CSkeletonPose::PreparePoseDataAndLocatorWriteables(Memory::CPool& memoryPool)
{
	if (m_pPoseDataWriteable)
	{
		m_poseDataWriteable.Initialize(GetPoseDataDefault());
		return true;
	}

	m_poseDataWriteable.AllocationRelease();
	m_poseDataWriteable.SetMemoryPool(&memoryPool);
	if (!m_poseDataWriteable.Initialize(m_poseData))
		return false;

	m_pPoseDataWriteable = &m_poseDataWriteable;

	return true;
}

void CSkeletonPose::SynchronizePoseDataAndLocatorWriteables()
{
	if (m_pPoseDataWriteable)
	{
		m_poseData.Initialize(m_poseDataWriteable);
		m_poseDataWriteable.AllocationRelease();
		m_pPoseDataWriteable = NULL;
	}
}

//////////////////////////////////////////////////////////////////////////
// reset the bone to default position/orientation
// initializes the bones and IK limb pose
//////////////////////////////////////////////////////////////////////////


void CSkeletonPose::SetDefaultPosePerInstance(bool bDataPoseForceWriteable)
{
	Skeleton::CPoseData& poseDataWriteable =
		bDataPoseForceWriteable ? GetPoseDataForceWriteable() : m_poseData;

	m_pSkeletonAnim->StopAnimationsAllLayers();

	m_bInstanceVisible=0;
	m_bFullSkeletonUpdate=0;

	PoseModifier::CRecoil* pRecoil = static_cast<PoseModifier::CRecoil*>(m_recoil.get());
	if (pRecoil)
		pRecoil->SetState(PoseModifier::CRecoil::State());
	
	m_LookIK().Init();
	
	if (m_PoseBlenderAim.get())
		GetPoseBlenderAim().m_blender.Init();
	//m_FootAnchor.Init();
	m_arrPostProcess.resize(0);

	uint32 numJoints = GetJointCount();
	if (numJoints)
	{
		for (uint32 i=0; i<numJoints; i++)
		{
			poseDataWriteable.m_pJointsRelative[i] = m_pModelSkeleton->m_poseData.m_pJointsRelative[i];
			poseDataWriteable.m_pJointsAbsolute[i] = m_pModelSkeleton->m_poseData.m_pJointsAbsolute[i];
		}

		for (uint32 i=1; i<numJoints; i++)
		{
			poseDataWriteable.m_pJointsAbsolute[i] = poseDataWriteable.m_pJointsRelative[i];
			int32 p = m_parrModelJoints[i].m_idxParent;
			if (p>=0)
				poseDataWriteable.m_pJointsAbsolute[i]	= poseDataWriteable.m_pJointsAbsolute[p] * poseDataWriteable.m_pJointsRelative[i];
			poseDataWriteable.m_pJointsAbsolute[i].q.Normalize();
		}
	}

	if ( m_pCharPhysics != NULL )
	{
		if (m_pInstance->m_pModel->m_ObjectType==CGA)
		{
			m_pInstance->UpdatePhysicsCGA(poseDataWriteable, 1, QuatTS(IDENTITY));
		}
		else if (!m_bPhysicsRelinquished)
		{
			SynchronizeWithPhysicalEntity(poseDataWriteable, m_pCharPhysics, Vec3(ZERO), Quat(IDENTITY), QuatT(IDENTITY), 0);
		}
	}
}

void CSkeletonPose::SetDefaultPoseExecute(bool bDataPoseForceWriteable)
{
	SetDefaultPosePerInstance(bDataPoseForceWriteable);

	IAttachmentManager* pIAttachmentManager = m_pInstance->GetIAttachmentManager();
	uint32 numAttachments = pIAttachmentManager->GetAttachmentCount();
	for (uint32 i=0; i<numAttachments; i++)
	{
		CAttachment* pAttachment = (CAttachment*)pIAttachmentManager->GetInterfaceByIndex(i);
		IAttachmentObject* pIAttachmentObject = pAttachment->m_pIAttachmentObject;
		if (pIAttachmentObject)
		{
			if (pAttachment->GetType()==CA_SKIN || pAttachment->GetType()==CA_PART)
				continue;
			if (pIAttachmentObject->GetAttachmentType() != IAttachmentObject::eAttachment_Character)
				continue;

			CCharInstance* pCharacterInstance = (CCharInstance*)pAttachment->m_pIAttachmentObject->GetICharacterInstance();
			pCharacterInstance->m_SkeletonPose.SetDefaultPosePerInstance(bDataPoseForceWriteable);	
		}
	}

	//	ExportSkeleton();

}

void CSkeletonPose::SetDefaultPose()
{
	m_bSetDefaultPoseExecute = true;
}

void CSkeletonPose::InitPhysicsSkeleton()
{

	assert(!m_arrPhysicsJoints.size());
	uint32 numJoints	= m_pInstance->m_pModel->m_ModelSkeleton.m_arrModelJoints.size();

	m_arrPhysicsJoints.resize(numJoints);
	for (uint32 i=0; i<numJoints; i++)
	{
		m_arrPhysicsJoints[i].m_DefaultRelativeQuat	=	m_pModelSkeleton->m_poseData.m_pJointsRelative[i];
		m_arrPhysicsJoints[i].m_qRelPhysParent[0] = m_parrModelJoints[i].m_qDefaultRelPhysParent[0];
		m_arrPhysicsJoints[i].m_qRelPhysParent[1] = m_parrModelJoints[i].m_qDefaultRelPhysParent[1];
	}
}

void CSkeletonPose::InitCGASkeleton()
{
	uint32 numJoints	= m_pInstance->m_pModel->m_ModelSkeleton.m_arrModelJoints.size();

	m_arrCGAJoints.resize(numJoints);

	for (uint32 i=0; i<numJoints; i++)
	{
		m_arrCGAJoints[i].m_CGAObjectInstance		= m_parrModelJoints[i].m_CGAObject;
	}

	m_Extents.Clear();
}



uint32 CSkeletonPose::GetChildrenCount(int parentID) const
{
	assert(parentID >= 0 && parentID < (int)m_poseData.GetJointCount());
	return m_parrModelJoints[parentID].m_numChildren;
}

int16 CSkeletonPose::GetChildID(int32 parentID, int32 childIndex) const
{
	assert(parentID >= 0 && parentID < (int)m_poseData.GetJointCount());
	assert(childIndex >= 0 && childIndex <  (int32)m_parrModelJoints[parentID].m_numChildren);
	return m_parrModelJoints[parentID].getChild(childIndex)->m_idx;
}

uint32 CSkeletonPose::GetJointCRC32 (int32 nJointID) const
{
	int32 numJoints = GetPoseDataDefault().GetJointCount();
	if(nJointID>=0 && nJointID<numJoints)
		return m_parrModelJoints[nJointID].m_nJointCRC32;
	assert(0);
	return ~0;
}



IStatObj* CSkeletonPose::GetStatObjOnJoint(int32 nId)
{
	if (nId < 0 || nId >= (int)GetPoseDataDefault().GetJointCount())
	{
		assert(0);
		return NULL;
	}
	if (m_arrCGAJoints.size())
	{
		const CCGAJoint& joint = m_arrCGAJoints[nId];
		return joint.m_CGAObjectInstance;
	}

	return 0;
}

void CSkeletonPose::SetStatObjOnJoint(int32 nId, IStatObj* pStatObj)
{
	if (nId < 0 || nId >= (int)GetPoseDataDefault().GetJointCount())
	{
		assert(0);
		return;
	}

	assert(m_arrCGAJoints.size());
	// do not handle physicalization in here, use IEntity->SetStatObj instead
	CCGAJoint& joint = m_arrCGAJoints[nId];  
	joint.m_CGAObjectInstance = pStatObj;  

	if(joint.m_pRNTmpData)
		gEnv->p3DEngine->FreeRNTmpData(&joint.m_pRNTmpData);
	assert(!joint.m_pRNTmpData);

	m_Extents.Clear();
}


void CSkeletonPose::SetPhysEntOnJoint(int32 nId, IPhysicalEntity *pPhysEnt)
{
	if (nId < 0 || nId >= (int)GetPoseDataDefault().GetJointCount())
	{
		assert(0);
		return;
	}
	if (!m_ppBonePhysics)
		memset(m_ppBonePhysics = new IPhysicalEntity*[GetPoseDataDefault().GetJointCount()], 0, GetPoseDataDefault().GetJointCount()*sizeof(IPhysicalEntity*));
	if (m_ppBonePhysics[nId])
		if (m_ppBonePhysics[nId]->Release()<=0)
			gEnv->pPhysicalWorld->DestroyPhysicalEntity(m_ppBonePhysics[nId]);
	m_ppBonePhysics[nId] = pPhysEnt;
	if (pPhysEnt)
		pPhysEnt->AddRef();
}

int CSkeletonPose::GetPhysIdOnJoint(int32 nId)
{
	if (nId < 0 || nId >= (int)GetPoseDataDefault().GetJointCount())
	{
		assert(0);
		return -1;
	}

	if (m_arrCGAJoints.size())
	{
		CCGAJoint& joint = m_arrCGAJoints[nId];

		if (joint.m_qqqhasPhysics>=0)
			return joint.m_qqqhasPhysics;
	}
	
	return -1;
}

void CSkeletonPose::DestroyPhysics()
{
	int i;
	if (m_pCharPhysics)
		g_pIPhysicalWorld->DestroyPhysicalEntity(m_pCharPhysics);
	m_pCharPhysics = 0;
	if (m_ppBonePhysics)
	{
		for(i=0;i<(int)GetPoseDataDefault().GetJointCount();i++)	if (m_ppBonePhysics[i])
			m_ppBonePhysics[i]->Release(), g_pIPhysicalWorld->DestroyPhysicalEntity(m_ppBonePhysics[i]);
		delete[] m_ppBonePhysics;
		m_ppBonePhysics = 0;
	}
	
	for(i=0;i<m_nAuxPhys;i++) {
		g_pIPhysicalWorld->DestroyPhysicalEntity(m_auxPhys[i].pPhysEnt);
		delete[] m_auxPhys[i].pauxBoneInfo;
		delete[] m_auxPhys[i].pVtx;
	}
	m_nAuxPhys = 0;
	m_bPhysicsRelinquished=m_bAliveRagdoll = 0;	m_timeStandingUp = -1;
}

void CSkeletonPose::SetAuxParams(pe_params* pf)
{
	for(int32 i=0; i<m_nAuxPhys; i++)
		m_auxPhys[i].pPhysEnt->SetParams(pf);
}

#include "PoseModifier/LimbIk.h"
uint32 CSkeletonPose::SetHumanLimbIK(const Vec3& vWorldPos, const char* strLimb)
{
	if (!m_limbIk.get())
	{
		::CryCreateClassInstance<IAnimationPoseModifier>("AnimationPoseModifier_LimbIk", m_limbIk);
		assert(m_limbIk.get());
	}

	Vec3 targetPositionLocal = m_pInstance->m_PhysEntityLocation.GetInverted() * vWorldPos;

	CLimbIk* pLimbIk = static_cast<CLimbIk*>(m_limbIk.get());
	pLimbIk->AddSetup(*(uint64*)strLimb, QuatT(targetPositionLocal, Quat(IDENTITY)));
	return 1;
}

uint32 CSkeletonPose::SetHumanLimbIK(const QuatT& vWorldPos, const char* strLimb, const bool adjustEndEffector)
{
	return SetHumanLimbIK(vWorldPos, *(uint64*)strLimb, adjustEndEffector);
}

uint32 CSkeletonPose::SetHumanLimbIK(const QuatT& vWorldPos, uint64 strLimb, const bool adjustEndEffector)
{
	if (!m_limbIk.get())
	{
		::CryCreateClassInstance<IAnimationPoseModifier>("AnimationPoseModifier_LimbIk", m_limbIk);
		assert(m_limbIk.get());
	}

	QuatT targetPositionLocal = m_pInstance->m_PhysEntityLocation.GetInverted() * vWorldPos;

	CLimbIk* pLimbIk = static_cast<CLimbIk*>(m_limbIk.get());
	pLimbIk->AddSetup(strLimb, targetPositionLocal, adjustEndEffector);
	return 1;
}

///////////////// Original HumanLimb IK, for immediate execution in case the results are needed right away ///////////////////////////
#include "PoseModifier/PoseModifierHelper.h"
uint32 CSkeletonPose::SetHumanLimbIK_ExecuteImmediately(const QuatT& wgoal, const char* limb, const bool adjustEndEffector)
{
	QuatT targetPositionLocal = m_pInstance->m_PhysEntityLocation.GetInverted() * wgoal;

	QuatT* const __restrict pRelativePose = SPU_PTR_SELECT( GetPoseDataExplicitWriteable().GetJointsRelative(), gSkeletonRelativePose );
	QuatT* const __restrict pAbsolutePose = SPU_PTR_SELECT( GetPoseDataExplicitWriteable().GetJointsAbsolute(), gSkeletonAbsolutePose );
	CModelSkeleton* __restrict pModelSkeleton = m_pModelSkeleton; 

	return PoseModifierHelper::IK_Solver(pModelSkeleton, limb, targetPositionLocal, pRelativePose, pAbsolutePose, adjustEndEffector);
}
uint32 CSkeletonPose::SetHumanLimbIK_ExecuteImmediately(const QuatT& wgoal, uint64 limb, const bool adjustEndEffector)
{
	return SetHumanLimbIK_ExecuteImmediately(wgoal, (const char*)&limb, adjustEndEffector);
}


#include "PoseModifier/GroundAlignment.h"
void CSkeletonPose::SetGroundAlignmentData(const bool bAlignSkeletonVertical, const f32 rootHeight, const Plane& planeLeft, const Plane& planeRight, int32 pelvisIDX) 
{
	if (!m_groundAlignment.get())
	{
		::CryCreateClassInstance<IAnimationGroundAlignment>("AnimationPoseModifier_GroundAlignment", m_groundAlignment);
		assert(m_groundAlignment.get());
	}

	CGroundAlignment* pGroundAlignment = static_cast<CGroundAlignment*>(m_groundAlignment.get());
	pGroundAlignment->SetData(
		bAlignSkeletonVertical, rootHeight, planeLeft, planeRight, pelvisIDX);
	m_bGroundAlign = true;
}

void CSkeletonPose::SetLookIK(uint32 ik, f32 FOR, const Vec3& vLookAtTarget,const f32 *customBlends, bool allowAdditionalTransforms, bool ignoreAimBlend)
{
		uint32 IsValid = vLookAtTarget.IsValid();

		if (ik)
		{
				if (IsValid==0)
				{
#if !defined(__SPU__)
						g_pISystem->Warning( VALIDATOR_MODULE_ANIMATION,VALIDATOR_WARNING,	VALIDATOR_FLAG_FILE,m_pInstance->GetFilePath(),	"CryAnimation: Look Target Invalid (%f %f %f)",vLookAtTarget.x,vLookAtTarget.y,vLookAtTarget.z );
#ifdef _DEBUG
						g_pISystem->debug_LogCallStack();
#endif
#endif
				}
		}

		//	float fColor[4] = {1,1,0,1};
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"LookIK: %d   %12.8f    Target:(%12.8f %12.8f %12.8f) ",ik,FOR, LookAtTarget.x, LookAtTarget.y, LookAtTarget.z );	g_YLine+=16.0f;

		m_LookIK().m_UseLookIK=ik;	
		m_LookIK().m_AllowAdditionalTransforms=allowAdditionalTransforms;
		m_LookIK().m_LookIKFOR=FOR;
		if (IsValid && ik)
				m_LookIK().m_LookIKTarget=vLookAtTarget;



		f32 t0=1.0f-GetPoseBlenderAim().m_blender.m_fDirIKBlend;
		f32 t1=GetPoseBlenderAim().m_blender.m_fDirIKBlend;

		if (ignoreAimBlend)
		{
				t0=1.0f;
				t1=0.0f;
		}

		//default values
		m_LookIK().m_lookIKBlends[0] = 0.04f*t0;
		m_LookIK().m_lookIKBlends[1] = 0.06f*t0;
		m_LookIK().m_lookIKBlends[2] = 0.08f*t0;
		m_LookIK().m_lookIKBlends[3] = 0.15f*t0;
		m_LookIK().m_lookIKBlends[4] = 0.60f*t0+0.25f*t1;

		if (customBlends)
		{
				cryMemcpy(m_LookIK().m_lookIKBlends,customBlends,sizeof(f32)*LOOKIK_BLEND_RATIOS);
				m_LookIK().m_lookIKBlends[0] = m_LookIK().m_lookIKBlends[0]*t0;
				m_LookIK().m_lookIKBlends[1] = m_LookIK().m_lookIKBlends[1]*t0;
				m_LookIK().m_lookIKBlends[2] = m_LookIK().m_lookIKBlends[2]*t0;
				m_LookIK().m_lookIKBlends[3] = m_LookIK().m_lookIKBlends[3]*t0;
				m_LookIK().m_lookIKBlends[4] = m_LookIK().m_lookIKBlends[4]*t0+0.25f*t1;
		}
}

void CSkeletonPose::ApplyRecoilAnimation(f32 fDuration, f32 fKinematicImpact, f32 fKickIn, uint32 nArms ) 
{
	PoseModifier::CRecoil* pRecoil = static_cast<PoseModifier::CRecoil*>(m_recoil.get());
	if (!pRecoil)
	{
		::CryCreateClassInstance<IAnimationPoseModifier>("AnimationPoseModifier_Recoil", m_recoil);
		pRecoil = static_cast<PoseModifier::CRecoil*>(m_recoil.get());
		assert(pRecoil);
	}

	PoseModifier::CRecoil::State state;
	state.time = 0.0f;
	state.duration = fDuration;
	state.strengh = fKinematicImpact; //recoil in cm
	state.kickin	= fKickIn; //recoil in cm
	state.arms = nArms; //1-right arm  2-left arm   3-both
	pRecoil->SetState(state);
};

//--------------------------------------------------------------------
//---              hex-dump the skeleton                           ---
//--------------------------------------------------------------------

void CSkeletonPose::ExportSkeleton()
{
/*
	FILE* pFile = ::fopen("e:/test.txt", "w");
	if (!pFile)
		return;

	::fprintf(pFile,
		"struct Joint\n"
		"{\n"
		"\tconst char* name;\n"
		"\tunsigned int nameCrc32;\n"
		"\tunsigned int nameCrc32Lowercase;\n"
		"\tsigned int parent;\n"
		"\tfloat tx, ty, tz;\n"
		"\tfloat qx, qy, qz, qw;\n"
		"};\n"
		"const Joint joints[] =\n"
		"{\n");

	uint32 jointCount = m_pModelSkeleton->GetJointCount();
	for (uint32 i=0; i<jointCount; ++i)
	{
		const char* name = m_pModelSkeleton->GetJointNameByID(i);
		int32 parent = m_pModelSkeleton->GetJointParentIDByID(i);
		uint32 crc32Normal = m_pModelSkeleton->m_arrModelJoints[i].m_nJointCRC32;
		uint32 crc32Lower = m_pModelSkeleton->m_arrModelJoints[i].m_nJointCRC32Lower;
		const QuatT& location = m_pModelSkeleton->m_poseData.GetJointsRelative()[i];
		::fprintf(pFile,
			"\t{ \"%s\", 0x%x, 0x%x, %d, %ff, %ff, %ff, %ff, %ff, %ff, %ff },\n",
			name, crc32Normal, crc32Lower, parent,
			location.t.x, location.t.y, location.t.z,
			location.q.v.x, location.q.v.y, location.q.v.z, location.q.w);
	}

	::fprintf(pFile, "};\n");
	::fclose(pFile);
*/
}

uint8 CSkeletonPose::IsLookIKEnabled()
{
	return m_lastPushedLookPose != NO_PUSHED_LOOKPOSE;
}
#include UNIQUE_VIRTUAL_WRAPPER(ISkeletonPose)
